#include <WProgram.h>
#include <AcceleroMMA7361.h>

AcceleroMMA7361::AcceleroMMA7361()
{
}

void AcceleroMMA7361::begin(int sleepPin, int selfTestPin, int zeroGPin, int xPin, int yPin, int zPin)
{
  pinMode(sleepPin, OUTPUT);
  pinMode(selfTestPin, OUTPUT);
  pinMode(zeroGPin, INPUT);
  pinMode(xPin, INPUT);
  pinMode(yPin, INPUT);
  pinMode(zPin, INPUT);
  digitalWrite(sleepPin,HIGH);
  digitalWrite(selfTestPin,LOW);
  _sleepPin = sleepPin;
  _selfTestPin = selfTestPin;
  _zeroGPin = zeroGPin;
  _xPin = xPin;
  _yPin = yPin;
  _zPin = zPin;
  setOffSets(0,0,0);
}

void AcceleroMMA7361::setOffSets(int xOffSet, int yOffSet, int zOffSet)
{
  _offSets[0]= xOffSet;
  _offSets[1]= yOffSet;
  _offSets[2]= zOffSet;
}

int AcceleroMMA7361::getXRaw()
{
  return analogRead(_xPin);
}

int AcceleroMMA7361::getYRaw()
{
  return analogRead(_yPin);
}

int AcceleroMMA7361::getZRaw()
{
  return analogRead(_zPin);
}

int AcceleroMMA7361::getXVolt()
{
  return _mapMMA7361V(analogRead(_xPin));
}

int AcceleroMMA7361::getYVolt()
{
  return _mapMMA7361V(analogRead(_yPin));
}

int AcceleroMMA7361::getZVolt()
{
  return _mapMMA7361V(analogRead(_zPin));
}

int AcceleroMMA7361::getXAccel()
{
  return _mapMMA7361G(analogRead(_xPin))+ _offSets[0];
}

int AcceleroMMA7361::getYAccel()
{
  return _mapMMA7361G(analogRead(_yPin)) + _offSets[1];
}

int AcceleroMMA7361::getZAccel()
{
  return _mapMMA7361G(analogRead(_zPin)) + _offSets[2];
}

int AcceleroMMA7361::_mapMMA7361V(int value)
{
  return map(value,0,1024,0,5000);
}

int AcceleroMMA7361::_mapMMA7361G(int value)
{
  return map(value,0,1024,-800,1600);
} 

void AcceleroMMA7361::callibrate()
{
  Serial.print("\nCallibrating MMA7361011");
  double var = 5000;
  double sumX = 0;
  double sumY = 0;
  double sumZ = 0;
  for (int i = 0;i<var;i++)
  {
    sumX = sumX + getXVolt();
    sumY = sumY + getYVolt();
    sumZ = sumZ + getZVolt();
    if (i%100 == 0)
    {
      Serial.print(".");
    }
  }
  setOffSets(1670 - sumX / var,1670 - sumY / var, + 1876 - sumZ / var);
  Serial.print("\nDONE");
}